# https://blog.csdn.net/weixin_42344264/article/details/106817912

from numpy import *
import numpy as np

if __name__ == '__main__':
    T_vi = np.mat([[9.999976e-01, 7.553071e-04, -2.035826e-03, -8.086759e-01],
                   [-7.854027e-04, 9.998898e-01, -1.482298e-02, 3.195559e-01],
                   [2.024406e-03, 1.482454e-02, 9.998881e-01, -7.997231e-01],
                   [0, 0, 0, 1]])

    # 10_03
    T_c0v = np.mat([[7.967514e-03, -9.999679e-01, -8.462264e-04, -1.377769e-02],
                    [-2.771053e-03, 8.241710e-04, -9.999958e-01, -5.542117e-02],
                    [9.999644e-01, 7.969825e-03, -2.764397e-03, -2.918589e-01],
                    [0, 0, 0, 1]])
    T_c0c1 = np.mat([[9.993440e-01, 1.814887e-02, -3.134011e-02, -5.370000e-01],
                     [1.842595e-02, 9.997935e-01, 8.575221e-03, 5.964270e-03],
                     [3.117801e-02, 9.147067e-03, 9.994720e-01, -1.274584e-02],
                     [0, 0, 0, 1]])

    T_vc1 = T_c0v.I * T_c0c1.I

    print("T_vc1:\n", T_vc1)
    T_ic0 = T_vi.I * T_c0v.I
    T_ic1 = T_vi.I * T_vc1
    print("T_ic0 is\n", T_ic0)
    print("T_ic1 is\n", T_ic1)


    T_ci = np.mat([      [-0.0007700462852280876, -0.9997424521143505, -0.02268119196830881, -0.0024210075723864097],
    [0.023094524431031216, 0.022657370010834954, -0.9994765062398894, 0.028743118037763114],
    [0.9997329893376643, -0.0012934545128397423, 0.02307112926148397, -0.14714691256197518],
    [0.0, 0.0, 0.0, 1.0]])
    print("T_ic is\n", T_ci.I)